%% 递推最小二乘vs卡尔曼滤波
% 实时估计位置和速度
% 分析估计误差
% 假设滤波初值无偏
% TODO: 尝试引入系统误差+滤波初值有偏的KF效果
clear all;
clc;

%% 参数初始化
% ENU坐标系：东X北Y天Z
T = 1;                          %测量周期(s)
Tf = 100;                       %测量总时间(s)
t = 0:T:Tf/T;                   %测量时间点(s)
pos0 = [0; 0; 50];              %初始位置(m)
vel0 = [0; 2.5*sqrt(3); 2.5];   %初始速度，CV模型(m/s)
muPos = [0; 0; 0];             %位置测量噪声均值
muVel = [0; 0; 0];             %速度测量噪声均值
sigmaPos = [3; 3; 4.5].^2;      %位置测量噪声方差(m)
sigmaVel = [0.1; 0.1; 0.15].^2; %速度测量噪声方差(m/s)
% 公用量
matrixHk = eye(6);
Ri = diag([sigmaPos; sigmaVel]);
Hi = matrixHk;
HiT = matrixHk';
invHTH = inv(matrixHk' * matrixHk);

%% 保证多种估计方法的测量值一致性，进行测量数据统一生成
% data[3, Tf/T + 1]
realPos = zeros(3, Tf/T + 1);
realVel = zeros(3, Tf/T + 1);
measurePos = zeros(3, Tf/T + 1);
measureVel = zeros(3, Tf/T + 1);
rng(1);
for i = 1:1:(Tf/T+1)
    realVel(:, i) = vel0;
    measureVel(:, i) = mvnrnd(realVel(:, i), diag(sigmaVel), 1);
    realPos(:, i) = pos0 + vel0 * t(i);
    measurePos(:, i) = mvnrnd(realPos(:, i), diag(sigmaPos), 1);
end
realPV = [realPos; realVel];
% 测量设备测位测速
measurePV = [measurePos; measureVel];

calVel = zeros(3, Tf/T + 1);
calVel(:, 1) = vel0;
for i = 2:1:(Tf/T+1)
    calVel(:, i) = (measurePos(:, i) - measurePos(:, i-1))./T;
end
% 测量设备只测位，速度靠计算推测
measurePcalV = [measurePos; calVel];

% 测试生成数据是否合理
% mean(measureVel', 1)
% var(measureVel', 1)
% testError1 = measurePos - realPos;
% mean(testError1', 1)
% var(testError1', 1)

%% 测量信息只有位置信息，进行位置速度实时估计
% 测量信息的后三项改用(X1-X0)/T推测
% 递推最小二乘(Z = HX+V)

estimateXpre = [pos0; vel0];                % ^X0
meanSquareErrorPre = inv(HiT * Ri * Hi);    % P0
estimatePV_P_LS = zeros(6, Tf/T + 1);
for i = 1:1:(Tf/T+1)
    tmp = Hi * meanSquareErrorPre * HiT + Ri;
    gainK = meanSquareErrorPre * HiT / tmp;
    Zi = measurePcalV(:, i);
    estimateXk = estimateXpre + gainK*(Zi - Hi*estimateXpre);
    meanSquareErrorK = meanSquareErrorPre - gainK*Hi*meanSquareErrorPre;
    estimateXpre = estimateXk;
    meanSquareErrorPre = meanSquareErrorK;
    % 记录
    estimatePV_P_LS(:, i) = estimateXk;
end

% 卡尔曼滤波(Xk = Phi*Xi + Gamma*Wi; Z = HX+V)
% 此处不考虑系统误差
matPHI = [eye(3), eye(3).*T; zeros(3), eye(3)];
matGAMMA = eye(6);
sigmaWi = 0;
matQ = [eye(3).*(T^3/3), eye(3).*(T^2/2); eye(3).*(T^2/2), eye(3).*T];
matQ = matQ.*sigmaWi;

estimateXpre = [pos0; vel0];                % ^X0
meanSquareErrorPre = inv(HiT * Ri * Hi);    % P0
estimatePV_P_KF = zeros(6, Tf/T + 1);
for i = 1:1:(Tf/T+1)
    estimateXk = matPHI * estimateXpre;
    meanSquareErrorK = matPHI*meanSquareErrorPre*matPHI' +...
        matGAMMA*matQ*matGAMMA';
    tmp = Hi * meanSquareErrorK * HiT + Ri;
    gainK = meanSquareErrorK * HiT / tmp;
    Zi = measurePcalV(:, i);
    estimateXk = estimateXk + gainK*(Zi - Hi*estimateXk);
    tmp = eye(6)-gainK*Hi;
    meanSquareErrorK = tmp*meanSquareErrorK*tmp' + gainK*Ri*gainK';
    estimateXpre = estimateXk;
    meanSquareErrorPre = meanSquareErrorK;
    % 记录
    estimatePV_P_KF(:, i) = estimateXk;
end

%% 测量信息包括位置和速度，进行位置速度实时估计
% 递推最小二乘(Z = HX+V)
estimateXpre = [pos0; vel0];                % ^X0
meanSquareErrorPre = inv(HiT * Ri * Hi);    % P0
estimatePV_PV_LS = zeros(6, Tf/T + 1);
for i = 1:1:(Tf/T+1)
    tmp = Hi * meanSquareErrorPre * HiT + Ri;
    gainK = meanSquareErrorPre * HiT / tmp;
    Zi = measurePV(:, i);
    estimateXk = estimateXpre + gainK*(Zi - Hi*estimateXpre);
    meanSquareErrorK = meanSquareErrorPre - gainK*Hi*meanSquareErrorPre;
    estimateXpre = estimateXk;
    meanSquareErrorPre = meanSquareErrorK;
    % 记录
    estimatePV_PV_LS(:, i) = estimateXk;
end

% 卡尔曼滤波(Xk = Phi*Xi + Gamma*Wi; Z = HX+V)
% 此处不考虑系统误差
matPHI = [eye(3), eye(3).*T; zeros(3), eye(3)];
matGAMMA = eye(6);
sigmaWi = 0;
matQ = [eye(3).*(T^3/3), eye(3).*(T^2/2); eye(3).*(T^2/2), eye(3).*T];
matQ = matQ.*sigmaWi;

estimateXpre = [pos0; vel0];                % ^X0
meanSquareErrorPre = inv(HiT * Ri * Hi);    % P0
estimatePV_PV_KF = zeros(6, Tf/T + 1);
for i = 1:1:(Tf/T+1)
    estimateXk = matPHI * estimateXpre;
    meanSquareErrorK = matPHI*meanSquareErrorPre*matPHI' +...
        matGAMMA*matQ*matGAMMA';
    tmp = Hi * meanSquareErrorK * HiT + Ri;
    gainK = meanSquareErrorK * HiT / tmp;
    Zi = measurePV(:, i);
    estimateXk = estimateXk + gainK*(Zi - Hi*estimateXk);
    tmp = eye(6)-gainK*Hi;
    meanSquareErrorK = tmp*meanSquareErrorK*tmp' + gainK*Ri*gainK';
    estimateXpre = estimateXk;
    meanSquareErrorPre = meanSquareErrorK;
    % 记录
    estimatePV_PV_KF(:, i) = estimateXk;
end

%% 绘图比较
for i = 1:1:6
    figure;
    plot(t, realPV(i, :),'--k', 'LineWidth', 1.5);
    hold on;
    plot(t, measurePV(i, :), 'LineWidth', 1);
    plot(t, estimatePV_P_LS(i, :), '--', 'LineWidth', 1.5);
    plot(t, estimatePV_P_KF(i, :), '--', 'LineWidth', 1.5);
    plot(t, estimatePV_PV_LS(i, :), 'LineWidth', 1.5);
    plot(t, estimatePV_PV_KF(i, :), 'LineWidth', 1.5);
    % legend('real','P-LS','P-KF');
    % legend('real','PV-LS','PV-KF');
    legend('real', 'measure','P-LS','P-KF','PV-LS','PV-KF');
    xlabel('t(s)')
    switch i
        case 1
            ylabel('posX(m)');
        case 2
            ylabel('posY(m)');
        case 3
            ylabel('posZ(m)');
        case 4
            ylabel('velX(m/s)');
        case 5
            ylabel('velY(m/s)');
        case 6
            ylabel('velZ(m/s)');
    end
end
